﻿using System;
using System.Collections.Generic;
using System.Text;

namespace SlabInspection
{
    class Geometry
    {
       /// <summary>
       /// 求两条直线的夹角p1,p2为第一条直线;p3,p4为第二条直线
       /// </summary>
       /// <param name="p1"></param>
       /// <param name="p2"></param>
       /// <param name="p3"></param>
       /// <param name="p4"></param>
       /// <returns>返回夹角,单位为弧度</returns>
        public static double GetCrossAngle(Point3D p1, Point3D p2, Point3D p3, Point3D p4)
        {
            double M1 = p2.x - p1.x;
            double N1 = p2.y - p1.y;
            double P1 = p2.z - p1.z;
            double M2 = p4.x - p3.x;
            double N2 = p4.y - p3.y;
            double P2 = p4.z - p3.z;

            return Math.Acos((M1 * M2 + N1 * N2 + P1 * P2) / (Math.Sqrt(M1 * M1 + N1 * N1 + P1 * P1) * Math.Sqrt(M2 * M2 + N2 * N2 + P2 * P2)));
        }

        public static double GetDistanceLine2Line(Point3D p1, Point3D p2, Point3D p3, Point3D p4)
        {
            double M1 = p2.x - p1.x;
            double N1 = p2.y - p1.y;
            double P1 = p2.z - p1.z;
            double M2 = p4.x - p3.x;
            double N2 = p4.y - p3.y;
            double P2 = p4.z - p3.z;

            return Math.Acos((M1 * M2 + N1 * N2 + P1 * P2) / (Math.Sqrt(M1 * M1 + N1 * N1 + P1 * P1) * Math.Sqrt(M2 * M2 + N2 * N2 + P2 * P2)));
        }

        /// <summary>
        /// 求过一已知点(p3)到空间直线(p1,p2)的焦点坐标
        /// </summary>
        /// <param name="p1"></param>
        /// <param name="p2"></param>
        /// <param name="p3"></param>
        /// <returns></returns>
        public static Point3D GetPoint2LineCrossPoint(Point3D p1, Point3D p2, Point3D p3)
        {
            //{m,n,p}已知直线的方向向量
            //t参数方程的参数
            double m = p2.x - p1.x;
            double n = p2.y - p1.y;
            double p = p2.z - p1.z;
            double t = -(m * (p1.x - p3.x) + n * (p1.y - p3.y) + p * (p1.z - p3.z)) / (m * m + n * n + p * p);
            Point3D P = new Point3D();
            P.x = m * t + p1.x;
            P.y = n * t + p1.y;
            P.z = p * t + p1.z;
            return P;
        }

        /// <summary>
        /// 空间两点距离
        /// </summary>
        /// <param name="p1"></param>
        /// <param name="p2"></param>
        /// <returns></returns>
        public static double Pt2Pt(Point3D p1, Point3D p2)
        {
            return Math.Sqrt((p2.x - p1.x) * (p2.x - p1.x) + (p2.y - p1.y) * (p2.y - p1.y) + (p2.z - p1.z) * (p2.z - p1.z));
        }

        /// <summary>
        /// 点到面的距离，设点坐标为P(x, y, z)，平面方程为ax+by+cz+d=0。
        /// </summary>
        /// <param name="p">空间任意一点</param>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <param name="c"></param>
        /// <param name="d"></param>
        /// <returns></returns>
        public static double DistPt2Panel(Point3D p, double a, double b, double c, double d)
        {
            return Math.Abs(a * p.x + b * p.y + c * p.z + d) / Math.Sqrt(a * a + b * b + c * c);
        }

        /// <summary>
        /// 已知3点，求平面方程
        /// </summary>
        /// <param name="p1"></param>
        /// <param name="p2"></param>
        /// <param name="p3"></param>
        /// <returns></returns>
        public static double[] PanelEquationFromThreePt(Point3D p1, Point3D p2, Point3D p3)
        {
            double a, b, c, d;
            double[] Plane = new double[4];
            a = ((p2.y - p1.y) * (p3.z - p1.z) - (p2.z - p1.z) * (p3.y - p1.y));
            b = ((p2.z - p1.z) * (p3.x - p1.x) - (p2.x - p1.x) * (p3.z - p1.z));
            c = ((p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x));
            d = (0 - (a * p1.x + b * p1.y + c * p1.z));
            Plane[0] = a;
            Plane[1] = b;
            Plane[2] = c;
            Plane[3] = d;
            return Plane;
        }

        /// <summary>
        /// 余弦定理求空间垂距
        /// </summary>
        /// <param name="A"></param>
        /// <param name="B"></param>
        /// <param name="C"></param>
        /// <returns></returns>
        public static double Pt2LineByTrigonometric(Point3D A, Point3D B, Point3D C)
        {
            //点AB是基准点，C点是参与计算的点，a是A点即A角的对边,点A B C 是有顺序要求的
            //计算三边的长
            double a = Geometry.Pt2Pt(C, B);//a边
            double b = Geometry.Pt2Pt(A, C);//b边
            double c = Geometry.Pt2Pt(A, B);//c边
            double d = 0.0;
            double cosB = (a * a + c * c - b * b) / (2 * a * c);//double cosB = (a * a + c * c - b * b) / 2 * a * c ;2008.12.11日上午用了一小时发现此低级错误.不要相信自己
            double cosA = (b * b + c * c - a * a) / (2 * b * c);
            if (cosA < Math.PI / 4.0 && cosA > 0)
            {
                d = Math.Sqrt(b * b - (cosA * b) * (cosA * b));
            }
            else
            {
                d = Math.Sqrt(a * a - (cosB * a) * (cosB * a));
            }
            //if (d > 500)
            //{
            //    MessageBox.Show("录入数据有问题");//;2008.12.11日下午用了一下午发现此低级错误:原始数据的顺序有问题。错误进，错误出
            //}
            if (double.IsNaN(d))
            {
                d = 0.01;

            }
            return d;
        }
    }
}
